/**
 * @file VirtualObsFollowPlanner.h
 * @author  
 * @brief 沿边规划(单沿边传感器)
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "data_struct/base/DataSet.h"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace behavior_controller
{

    class VirtualObsFollowPlanner
    {
    public:
        /**
         * @brief 沿边规划的构造函数类
         * @return
         */
        VirtualObsFollowPlanner(){};

        /**
         * @brief 更新沿边距离
         * @return
         */
        void UpdateObsDistance(double dis) { dis_ = dis; };

        /**
         * @brief 沿边规划的启动运行函数
         * @return
         */
        void run(double &linear_vel, double &angular_vel,bool &finished);

        /**
         * @brief 沿障碍物时的PID调节函数
         * @return
         */
        void pid_adjustment(double &linear_vel, double &angular_vel);

        double last_dis_p; //记录上一次的距离偏差，用于计算微分环节的偏差之差
        //PoseF start_pose;
        geometry_msgs::msg::PoseStamped start_pose;
        bool finished = true; 
    private:
        double dis_; //传感器反馈的沿边距离
    };

    //static 
    extern VirtualObsFollowPlanner virtual_obs_follow_planner; //实例化沿障碍物类
} // namespace behavior_controller